# 研究机构：上海大学机自学院
# 研 究 生：王强
# 开发时间：2024/6/13 14:00
import math

import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
# 导入MN逻辑法模块
import New_MN_Logic
import GNN_MTT
import EKF_MTT
import copy
# 真实路径
if __name__ == '__main__':
    path = './x_ture.xlsx'
    data_x = pd.read_excel(path, header=None, skiprows=[0])
    x_ture = data_x.values

    path = './y_ture.xlsx'
    data_y = pd.read_excel(path, header=None, skiprows=[0])
    y_ture = data_y.values

    path = './ath_obs.xlsx'
    data_ath_obs = pd.read_excel(path, header=None, skiprows=[0])
    ath_obs = data_ath_obs.values

    path = './dis_obs.xlsx'
    data_dis_obs = pd.read_excel(path, header=None, skiprows=[0])
    dis_obs = data_dis_obs.values
    scans = []
    M = 4
    N = 3
    tracker = New_MN_Logic.TrackInitiation(threshold=100, M=M, N=N)
    Initial_P = np.diag(np.ones(4)) * 0.01  # 初始P设为0 ??? zeros(4, 4)
    Initial_S = np.diag(np.ones(4))
    last_ekf_P_S = []                                                                      # 滤波过程中需要用到的上一时刻状态
    associate_tracks = []                                                                  # 经过全局最近邻数据关联算法的到累计关联航迹库
    ekf_z_tracks = []                                                                      # 经过滤波后得到的目标航迹库
    for step in range(len(data_y)):
        # 雷达累计扫描M次后，累计连续扫描数据组数等于M次对航迹进行起始
        if len(scans) == M:
            print(step)
            # tracker.track_id =
            # 调用航迹起始模块对航迹进行起始
            initiated_tracks = tracker.start_tracks(scans)                                                     # 起始航迹
            # 航迹管理，将新起始的航迹存入到关联航迹库中，进行目标跟踪
            start_target_ids = [dict_entry[next(iter(dict_entry))] for dict_entry in tracker.starting_tracks]  # 取出所有起始航迹所属目标的id值
            associate_ids = [dict_entry[next(iter(dict_entry))] for dict_entry in associate_tracks]            # 取起始航迹中已经进行关联的目标的id值
            # last_target_ids = [dict_entry[next(iter(dict_entry))] for dict_entry in last_ekf_P_S]
            # 遍历起始航迹中的每一条航迹，将新起始的航迹存入到关联航迹库中，以便于进行目标跟踪
            for t in range(len(tracker.starting_tracks)):
                # print(tracker.starting_tracks[t])
                # 将新起始的航迹存入到关联航迹库和航迹跟踪库，所起始的新航迹赋予初始的状态转移矩阵和协方差矩阵
                if start_target_ids[t] not in associate_ids:
                    # 向关联航迹库存入新的起始航迹
                    associate_tracks.append({
                        'id': start_target_ids[t],
                        "associate_points": tracker.starting_tracks[t]['points']
                    })
                    # 存入初始滤波算法所用的状态
                    # print(tracker.starting_tracks[t]['points'])
                    Z_obs_datas = []
                    for i in range(len(tracker.starting_tracks[t]['points'])):
                        intial_z_data = tracker.starting_tracks[t]['points'][i]
                        # print('intial_z_data', intial_z_data)
                        x_obs = intial_z_data[0] * np.cos(np.array(intial_z_data[1]) / 180 * 3.14)
                        y_obs = intial_z_data[0] * np.sin(np.array(intial_z_data[1]) / 180 * 3.14)
                        if i == 0:
                            ekf_mtt = EKF_MTT.EKF(px=x_obs, vx=0, py=y_obs, vy=0, w=0.01, T=1)
                            last_ekf_P_S.append({
                                'id': start_target_ids[t],
                                'P': ekf_mtt.P,
                                'S': ekf_mtt.S,
                                'X': ekf_mtt.x,
                                'Next_Z': ekf_mtt.pre_nextz
                            })
                        ekf_mtt.z = np.transpose([x_obs, y_obs])
                        ekf_mtt.P = last_ekf_P_S[t]['P']
                        ekf_mtt.x = last_ekf_P_S[t]['X']
                        ekf_mtt.KF_update()
                        # 存储中间状态（状态协方差方程P，S 和预测状态X）
                        last_ekf_P_S[t]['P'] = ekf_mtt.P
                        last_ekf_P_S[t]['X'] = ekf_mtt.x
                        last_ekf_P_S[t]['S'] = ekf_mtt.S
                        last_ekf_P_S[t]['Next_Z'] = ekf_mtt.pre_nextz
                        Z_obs_datas.append([np.linalg.norm([ekf_mtt.x[0], ekf_mtt.x[2]], ord=2),
                                            math.atan2(ekf_mtt.x[2], ekf_mtt.x[0]) / 3.14 * 180])
                        # 目标航迹库存入起始的航迹
                    ekf_z_tracks.append({
                        'id': start_target_ids[t],
                        "ekf_points": Z_obs_datas
                    })

            scans.pop(0)            # 删除M次雷达扫描周期内的最早一批点迹

        if step < M:
            z_obs_datas = np.transpose([dis_obs[step], ath_obs[step]])
            scans.append(z_obs_datas)
        else:

            targets = []                                                 # 获取下一时刻的雷达量测值
            S_datas = []                                                 # 协方差矩阵
            z_obs_datas = np.transpose([dis_obs[step], ath_obs[step]])
            # 全局最近邻数据关联算法，获得已关联的航迹
            for t in range(len(ekf_z_tracks)):
                targets.append(last_ekf_P_S[t]['Next_Z'])
                S_datas.append(last_ekf_P_S[t]['S'])
            gnn_mtt = GNN_MTT.GNN(targets=targets, measurements=z_obs_datas)
            gnn_mtt.S = S_datas
            associate_tracks = gnn_mtt.global_nearest_neighbor(associate_tracks)                # tracker.tracks为实际观测的航迹，并未进行卡尔曼滤波更新得到准确的卡尔曼滤波
            # print(gnn_mtt.association_matrix)
            row_sums = gnn_mtt.association_matrix.sum(axis=1)                               # 关联矩阵 association_matrix 按行求和， 行为零对应的目标无量测关联，表示出现漏检
            # print(row_sums)
            # print(associated_track)
            for t in range(len(row_sums)):
                # len(associated_track[t]) == 1 表示当前时刻航迹无关联雷达量测，出现漏检，扩展生命周期管理方法，对航迹进行删除和管理
                # print(associated_track[t])
                if row_sums[t] == 0:
                    print(1)
                else:
                    z_obs_data = associate_tracks[t]['associate_points'][-1]
                    x_obs = z_obs_data[0] * np.cos(np.array(z_obs_data[1]) / 180 * 3.14)
                    y_obs = z_obs_data[0] * np.sin(np.array(z_obs_data[1]) / 180 * 3.14)
                    ekf_mtt.z = [x_obs, y_obs]
                    ekf_mtt.P = last_ekf_P_S[t]['P']
                    ekf_mtt.x = last_ekf_P_S[t]['X']
                    ekf_mtt.KF_update()
                    # 存储中间状态（状态协方差方程P，S 和预测状态X）
                    last_ekf_P_S[t]['P'] = ekf_mtt.P
                    last_ekf_P_S[t]['X'] = ekf_mtt.x
                    last_ekf_P_S[t]['S'] = ekf_mtt.S
                    last_ekf_P_S[t]['Next_Z'] = ekf_mtt.pre_nextz

                    ekf_z_tracks[t]["ekf_points"] += [np.linalg.norm([ekf_mtt.x[0], ekf_mtt.x[2]], ord=2),
                                                      math.atan2(ekf_mtt.x[2], ekf_mtt.x[0]) / 3.14 * 180]

    print('ekf_z_tracks=', ekf_z_tracks)
    print(associate_tracks)
